// Fitting result#5 of c_2 and sigma^2
#include "Signal.h"
#include "MPC_EW2D_PatternedW_c.h"

// -----------------------------------------------------------
// C2 = C2(W)
// C2: nm^2/s
// W: nm/s
// 
// c2(W): rational function, constant numerator and quadratic denominator
// c2(W) = pc1*x^3+pc2*x^2+pc3*x+pc4
// dc2/dW = 3*pc1*x^2+2*pc2*x+pc3
const double pc1 = -10.53;
const double pc2 = 12.23;
const double pc3 = -4.953;
const double pc4 = 0.8804;

eq_C2::eq_C2(int i_LatticeSize,double i_XYmax):MathFunction(1,0),LatticeSize(i_LatticeSize),XYmax(i_XYmax){
}

eq_C2::~eq_C2(){}

double eq_C2::value(const double *x){
  double W = x[0];
  return pc1*W*W*W+pc2*W*W+pc3*W+pc4;
}

void eq_C2::gradient(const double *x,double *grad){
  double W = x[0];
  grad[0] = 3*pc1*W*W+2*pc2*W+pc3;
}

bool eq_C2::parameterize(const double *para){
  //  LatticeSize = int(para[0]);
  return true;
}

bool eq_C2::parameterize(int para_ID,double para_val){
  //  switch(para_ID){
  //  case 0:
  //    LatticeSize = int(para_val);break;
  //  default:
  //    cerr << "eq_C2::parameterize(): invalid PARA_ID\n";
  //  }
  return true;
}

// ------------------------------------------------------------
// Math function:
// sigma2 = sigma2(W)
// sigma2: nm^2/s^2
// W: nm/s
//
// sigma^2(W): Quadratic polynomial
// sigma^2(W) = ps1*x^4+ps2*x^3+ps3*x^2+ps4*x+ps5
// dsigma^2/dW = 4*ps1*x^4+3*ps2*x^2+2*ps3*x+ps4
const double ps1 = -15.98;
const double ps2 = 16.39;
const double ps3 = -5.634;
const double ps4 = 0.8773;
const double ps5 = 0.01836;

//eq_sigma2::eq_sigma2(int i_LatticeSize):MathFunction(1,0),LatticeSize(i_LatticeSize){
//}

eq_sigma2::eq_sigma2(int i_LatticeSize, double i_XYmax):MathFunction(1,0),LatticeSize(i_LatticeSize),XYmax(i_XYmax){}

eq_sigma2::~eq_sigma2(){}

double eq_sigma2::value(const double *x){
  double W = x[0];
  return ps1*W*W*W*W+ps2*W*W*W+ps3*W*W+ps4*W+ps5;
}

void eq_sigma2::gradient(const double *x, double *grad){
  double W = x[0];
  grad[0] = 4*ps1*W*W*W+3*ps2*W*W+2*ps3*W+ps4;
}

bool eq_sigma2::parameterize(const double *para){
  //  LatticeSize = int(para[0]);
  return true;
}

bool eq_sigma2::parameterize(int para_ID, double para_val){
  //  switch(para_ID){
  //  case 0:
  //    LatticeSize = int(para_val);
  //    break;
  //  default:
  //    cerr << "eq_sigma2::parameterize(): invalid PARA_ID\n";
  //  }
  return true;
}
// --------------------------------------------------------------
// cov_z_{p}(W)
/*
eq_cov_z_p::eq_cov_z_p(int i_p,int i_m,int i_n,
		       double i_XYmax,int i_LatticeSize,
		       double i_dt, double i_cov_z_m):
  MathFunction(1,7),p(i_p),m(i_m),n(i_n),
  LatticeSize(i_LatticeSize),XYmax(i_XYmax),
  dt(i_dt),cov_z_m(0),
  f_c2(i_LatticeSize,i_XYmax),
  f_sigma2(i_LatticeSize,i_XYmax)
{}

eq_cov_z_p::~eq_cov_z_p(){}

double eq_cov_z_p::value(const double *x){
  double W = x[0];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);

  if(m==0 && n==0){
    if(p==1){
      return sigma2*dt;
    }
    else
      return 0.0;
  }
  else if((m==0 || n==0) && (p==0)){
    return 0.0;
  }
  else if((m==0) && (p==2)){
    return 0.0;
  }
  else if((n==0) && (p==3)){
    return 0.0;
  }
  else{
    double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
    double temp1 = exp(2*lambda*dt);
    double temp2 = sigma2*(temp1-1)/(2*lambda);
    double cov_z_p = cov_z_m*temp1+temp2;
    return cov_z_p;
  }
}


void eq_cov_z_p::gradient(const double *x,double *grad){
  double W = x[0];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);

  double grad_C2[2];
  double grad_sigma2[2];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);

  if(m==0 && n==0){
    if(p==1){
      grad[0] = dt*grad_sigma2[0];
      return;
    }
    else{
      grad[0] = 0.0;
      return;
    }
  }
  else if((m==0 || n==0) && (p==0)){
    grad[0] = 0.0;
    return;
  }
  else if((m==0) && (p==2)){
    grad[0] = 0.0;
    return;
  }
  else if((n==0) && (p==3)){
    grad[0] = 0.0;
    return;
  }
  else{
    double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
    double temp1 = exp(2*lambda*dt);
    double dcov_z_dlambda = 2*temp1*cov_z_m*dt
      +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
    double dlambda_dC2 = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax);
    double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
    double dcov_z_dsigma2 = (temp1-1)/(2*lambda);
    // dcov_z_dW
    grad[0]= grad_C2[0]*dcov_z_dC2+grad_sigma2[0]*dcov_z_dsigma2;
#ifdef DEBUG
    cout << "grad_C2[0]:" << grad_C2[0] << "\n";
    cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
    cout << "dcov_z_dC2:" << dcov_z_dC2 << "\n";
    cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
#endif
    return;
  }
}

bool eq_cov_z_p::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    n = int(para_val);break;
  case 3:
    dt = para_val;break;
  case 4:
    cov_z_m = para_val;break;
  case 5:
    XYmax = para_val;break;
  case 6:
    LatticeSize = int(para_val);break;
    f_c2.parameterize(0,LatticeSize);
    f_sigma2.parameterize(0,LatticeSize);
  default:
    cerr << "eq_cov_z_p::parameterize(): invalid PARA_ID\n";
    exit(-1);
  }
  return true;
}*/
// -------------------------------------------------------------------
// z2_p(W,A)
eq_z2_p::eq_z2_p(int i_p,int i_m,int i_n,
		 double i_dt,double i_cov_z_m,double i_mean_z_m,
		 double i_Freq,double i_XYmax,int i_LatticeSize):
  MathFunction(2,9),p(i_p),m(i_m),n(i_n),
  dt(i_dt),cov_z_m(i_cov_z_m),mean_z_m(i_mean_z_m),
  Freq(i_Freq),XYmax(i_XYmax),LatticeSize(i_LatticeSize),
  f_c2(i_LatticeSize,i_XYmax),f_sigma2(i_LatticeSize,i_XYmax)
{}

eq_z2_p::~eq_z2_p(){}

double eq_z2_p::value(const double *x){
  double W = x[0];
  double A = x[1];
  double cov_z_p;
  double mean_z_p;
  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pmn = eq_W_pmn(W,A);

  if(m==0 && n==0){
    if(p==1){
      cov_z_p = dt*sigma2*dt;
      mean_z_p = dt*w_pmn;
    }
    else{
      cov_z_p = 0.0;
      mean_z_p = 0.0;
    }
  }
  else if((m==0 || n==0) && (p==0)){
    cov_z_p = 0.0;
    mean_z_p = 0.0;
  }
  else if((m==0) && (p==2)){
    cov_z_p = 0.0;
    mean_z_p = 0.0;
  }
  else if((n==0) && (p==3)){
    cov_z_p = 0.0;
    mean_z_p = 0.0;
  }
  else{
    double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
    double temp1 = exp(2*lambda*dt);
    double temp2 = sigma2*(temp1-1)/(2*lambda);
    double temp3 = exp(lambda*dt);
    double temp4 = (temp3-1)/lambda;

    cov_z_p = cov_z_m*temp1+temp2;
    mean_z_p = mean_z_m*temp3+w_pmn*(temp3-1)/lambda;
  }
  return cov_z_p+mean_z_p*mean_z_p;
}

void eq_z2_p::gradient(const double *x,double *grad){
  double W = x[0];
  double A = x[1];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pmn = eq_W_pmn(W,A);
  double grad_C2[2];
  double grad_sigma2[2];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);

  if(m==0 && n==0){
    if(p==1){
      grad[0] = dt*grad_sigma2[0];
      grad[1] = 0.0;
      return;
    }
    else{
      grad[0] = 0.0;
      grad[1] = 0.0;
      return;
    }
  }
  else if((m==0||n==0) && (p==0)){
    grad[0] = 0.0;
    grad[1] = 0.0;
    return;
  }
  else if((m==0) && (p==2)){
    grad[0] = 0.0;
    grad[1] = 0.0;
    return;
  }
  else if((n==0) && (p==3)){
    grad[0] = 0.0;
    grad[1] = 0.0;
    return;
  }
  else{
    double lambda= -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
    double temp1 = exp(2*lambda*dt);
    double temp2 = sigma2*(temp1-1)/(2*lambda);
    double temp3 = exp(lambda*dt);
    double temp4 = (temp3-1)/lambda;
    double mean_z_p = mean_z_m*temp3+w_pmn*temp4;
    
    double dw_pmn_dW;
    if(p==1&&m==0&&n==0){
      dw_pmn_dW = XYmax;
    }
    else{
      dw_pmn_dW = 0.0;
    }
    double dw_pmn_dA = eq_dW_pmn_dA();

    double dlambda_dC2 = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax);
    double dlambda_dW = dlambda_dC2*grad_C2[0];
    double dcov_z_dlambda =  2*temp1*cov_z_m*dt
      +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
    double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
    double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

    double dcov_z_dW = dcov_z_dC2*grad_C2[0]+dcov_z_dsigma2*grad_sigma2[0];
    double dcov_z_dA = 0.0;  

    double dmean_z_dlambda = temp3*mean_z_m*dt
      +w_pmn*(lambda*temp3*dt-(temp3-1))/(lambda*lambda);
    double dmean_z_dw_pmn  = (temp3-1)/lambda;
    double dmean_z_dW = dmean_z_dlambda*dlambda_dW+dmean_z_dw_pmn*dw_pmn_dW;
    double dmean_z_dA = dmean_z_dw_pmn*dw_pmn_dA;

    grad[0]= dcov_z_dW+2*mean_z_p*dmean_z_dW;
    grad[1]= dcov_z_dA+2*mean_z_p*dmean_z_dA;
    
#ifdef DEBUG
    cout << "********************************************\n";
    cout << "[p,m,n] = [" << p << ", "  << m << ", " << n << "]\n";
    cout << "grad_C2[0]    :" << grad_C2[0] << "\n";
    cout << "grad_C2[1]    :" << grad_C2[1] << "\n";
    cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
    cout << "grad_sigma2[1]:" << grad_sigma2[1] << "\n";
    cout << "dcov_z_dC2    :" << dcov_z_dC2 << "\n";
    cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
#endif
    return;
  }
}

bool eq_z2_p::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    n = int(para_val);break;
  case 3:
    dt = para_val;break;
  case 4:
    cov_z_m = para_val;break;
  case 5:
    mean_z_m = para_val;break;
  case 6:
    Freq = para_val;break;
  case 7:
    XYmax = para_val;break;
  case 8:
    LatticeSize = int(para_val);
    f_c2.parameterize(0,LatticeSize);
    f_sigma2.parameterize(0,LatticeSize);
    break;
  default:
    cerr << "eq_z2_p::parameterize(): invalid para_ID\n";
  }
  return true;
}

bool eq_z2_p::parameterize(const double *para){
  p = int(para[0]);
  m = int(para[1]);
  n = int(para[2]);
  dt = para[3];
  cov_z_m = para[4];
  mean_z_m = para[5];
  Freq = para[6];
  XYmax = para[7];
  LatticeSize = int(para[8]);

  return true;
}

double eq_z2_p::eq_W_pmn(double W,double A){
  double w_pmn;
  switch(p){
  case 0:
    w_pmn = 0;
    break;
  case 1:
    if(n!=0){
      w_pmn = 0.0;
    }
    else{
      if(m==0){
	// w_pmn = W*XYmax+A*XYmax/(2*Freq*pi)*(1-cos(2*Freq*pi));
	w_pmn = W*XYmax;
      }
      else{
	if(m==Freq){
	  w_pmn = 0.0;
	}
	else{
	  // w_pmn = SQRT_TWO*A*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
	  w_pmn = 0.0;
	}
      }
    }
    break;
  case 2:
 //   if(m!=0&&n!=0){
 //     if(m==Freq){
	//w_pmn = SQRT_TWO*A*XYmax/2;
 //     }
 //     else{
	//// w_pmn = SQRT_TWO*XYmax*A*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
	//w_pmn = 0.0;
 //     }
 //   }
 //   else{
 //     w_pmn = 0.0;
 //   }
    if(m==Freq && n==0){
      w_pmn = SQRT_TWO*A*XYmax/2;
    }
    else{
      w_pmn = 0.0;
    }
    break;
  case 3:
    w_pmn= 0.0;
    break;
  }
  return w_pmn;
}

double eq_z2_p::eq_dW_pmn_dA(){
  double dW_dA;
  switch(p){
  case 0:
    dW_dA = 0; 
    break;
  case 1:
    if(m==0&&n==0){
      // dW_dA = XYmax*(1-cos(2*Freq*pi))/(2*Freq*pi);
      dW_dA = 0.0;
    }
    else if(m!=Freq && n==0){
      // dW_dA = SQRT_TWO*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
      dW_dA = 0.0;
    }
    else{
      dW_dA = 0.0;
    }
    break;
  case 2:
 //   if(m!=0&&n!=0){
 //     if(m==Freq){
	//dW_dA = SQRT_TWO*XYmax/2;
 //     }
 //     else{
	//// dW_dA = SQRT_TWO*XYmax*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
	//dW_dA = 0.0;
 //     }
 //   }
 //   else{
 //     dW_dA = 0.0;
 //   }
    if(m==Freq && n==0){
      dW_dA = SQRT_TWO*XYmax/2;
    }
    else{
      dW_dA = 0.0;
    }
    break;
  case 3:
    dW_dA = 0.0; 
    break;
  }
  return dW_dA;
}  
// ---------------------------------------------------------------
eq_r2_f::eq_r2_f(double i_dt,int i_mode,double i_XYmax,
		 int i_LatticeSize,double i_Freq):
  MathFunction(2,5),
  dt(i_dt),mode(i_mode),XYmax(i_XYmax),
  LatticeSize(i_LatticeSize),Freq(i_Freq),
  z2_p(0,0,0,i_dt,0,0,i_Freq,i_XYmax,i_LatticeSize)
{
  int i;
  int dim = (mode+1)*(mode+1);
  for(i=0;i<4;i++){
    cov_z_m[i] = NULL;
    mean_z_m[i] = NULL;
  }
}

eq_r2_f::~eq_r2_f(){
#ifdef DEBUG
  cout << "~eq_r2_f()\n";
#endif
}

double eq_r2_f::value(const double *x){
  int    p,m,n;
  double r2_f = 0.0;
  int    index;
  double para_z2_p[9];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  para_z2_p[7] = XYmax;
  para_z2_p[8] = LatticeSize;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  r2_f += z2_p.value(x);
	}
      }
    }
  }
  return r2_f/=(XYmax*XYmax);
}

void eq_r2_f::gradient(const double *x,double *grad){
  int    p,m,n;
  int    index;
  double para_z2_p[9];
  double grad_z2_p[2];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  para_z2_p[7] = XYmax;
  para_z2_p[8] = LatticeSize;
  grad[0] = 0.0;
  grad[1] = 0.0;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  z2_p.gradient(x,grad_z2_p);
	  //	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	  //	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	  grad[0] += grad_z2_p[0];
	  grad[1] += grad_z2_p[1];
	}
      }
    }
  }
  grad[0] /= (XYmax*XYmax);
  grad[1] /= (XYmax*XYmax);
}

bool eq_r2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    mode = int(para_val);
    break;
  case 1:
    XYmax = para_val;
    z2_p.parameterize(7,XYmax);
    break;
  case 2:
    Freq = para_val;
    z2_p.parameterize(6,Freq);
    break;
  case 3:
    LatticeSize = int(para_val);
    z2_p.parameterize(8,LatticeSize);
    break;
  case 4:
    dt = para_val;
    z2_p.parameterize(3,dt);
    break;
  default:
    cerr << "er_r2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_r2_f::parameterize(int para_ID,double *para_array){
  switch(para_ID){
  case 0:
    cov_z_m[0] = para_array;break;
  case 1:
    mean_z_m[0] = para_array;break;
  case 2:
    cov_z_m[1] = para_array;break;
  case 3:
    mean_z_m[1] = para_array;break;
  case 4:
    cov_z_m[2] = para_array;break;
  case 5:
    mean_z_m[2] = para_array;break;
  case 6:
    cov_z_m[3] = para_array;break;
  case 7:
    mean_z_m[3] = para_array;break;
  default:
    cerr << "ef_f::parameterize(int, double*): invalid PARA_ID\n";
  }
  return true;
}
// ---------------------------------------------------------------
eq_m2_f::eq_m2_f(double i_dt,int i_mode,double i_XYmax,
		 int i_LatticeSize, double i_Freq):
  MathFunction(2,5),
  dt(i_dt),mode(i_mode),XYmax(i_XYmax),
  LatticeSize(i_LatticeSize),Freq(i_Freq),
  z2_p(0,0,0,i_dt,0,0,i_Freq,i_XYmax,i_LatticeSize)
{
  int dim = (mode+1)*(mode+1);
  for(int i=0;i<4;i++){
    cov_z_m[i] = NULL;
    mean_z_m[i] = NULL;
    K[i] = new double[dim];
  }
  CalculateK();
}

eq_m2_f::~eq_m2_f(){
  cout << "~eq_m_f()\n";
  int i;
  for(i=0;i<4;i++){
    delete []K[i];
  }
}

double eq_m2_f::value(const double *x){
  int    p,m,n;
  double m2_f = 0.0;
  int    index;
  double para_z2_p[9];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  para_z2_p[7] = XYmax;
  para_z2_p[8] = LatticeSize;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  m2_f += K[p][index]*z2_p.value(x);
	}
      }
    }
  }
  return m2_f;
}

void eq_m2_f::gradient(const double *x,double *grad){
  int    p,m,n;
  int    index;
  double para_z2_p[9];
  double grad_z2_p[2];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  para_z2_p[7] = XYmax;
  para_z2_p[8] = LatticeSize;
  grad[0] = 0.0;
  grad[1] = 0.0;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  z2_p.gradient(x,grad_z2_p);
	  //	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	  //	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	  grad[0] += K[p][index]*grad_z2_p[0];
	  grad[1] += K[p][index]*grad_z2_p[1];
	}
      }
    }
  }
}  

bool eq_m2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    mode = int(para_val);break;
  case 1:
    XYmax = para_val;
    z2_p.parameterize(7,XYmax);
    CalculateK();
    break;
  case 2:
    Freq = para_val;
    z2_p.parameterize(6,Freq);
    break;
  case 3:
    LatticeSize = int(para_val);
    z2_p.parameterize(8,LatticeSize);
    CalculateK();
    break;
  case 4:
    dt = para_val;
    z2_p.parameterize(3,dt);
    break;
  default:
    cerr << "er_m2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_m2_f::parameterize(int para_ID,double *para_array){
  switch(para_ID){
  case 0:
    cov_z_m[0] = para_array;break;
  case 1:
    mean_z_m[0] = para_array;break;
  case 2:
    cov_z_m[1] = para_array;break;
  case 3:
    mean_z_m[1] = para_array;break;
  case 4:
    cov_z_m[2] = para_array;break;
  case 5:
    mean_z_m[2] = para_array;break;
  case 6:
    cov_z_m[3] = para_array;break;
  case 7:
    mean_z_m[3] = para_array;break;
  default:
    cerr << "ef_f::parameterize(int, double*): invalid PARA_ID\n";
  }
  return true;
}

void eq_m2_f::CalculateK(){
  for(int i=0;i<4;i++){
    for(int m=0;m<=mode;m++){
      double temp = 4*sin(pi*m/LatticeSize)*sin(pi*m/LatticeSize)/(XYmax*XYmax);
      for(int n=0;n<=mode;n++){
	int index = m*(mode+1)+n;
	switch(i){
	case 0:
	  if(m==0||2*m==LatticeSize||n==0||2*n==LatticeSize){
	    K[i][index] = 0.0;
	  }else{
	    K[i][index] = temp;
	  }
	  break;
	case 1:
	  if(m==0){
	    K[i][index] = 0.0;
	  }else{
	    K[i][index] = temp;
	  }
	  break;
	case 2:
	  if(m==0||2*m==LatticeSize){
	    K[i][index] = 0.0;
	  }else{
	    K[i][index] = temp;
	  }
	  break;
	case 3:
	  if(n==0||2*n==LatticeSize||m==0){
	    K[i][index] = 0.0;
	  }else{
	    K[i][index] = temp;
	  }
	}
      }
    }
  }
}
// ---------------------------------------------------------------
eq_f::eq_f(double i_r2_set,double i_q_r,
	   double i_m2_set,double i_q_m,
	   double i_dt, int i_mode,int i_LatticeSize,double i_XYmax,double i_Freq):
  MathFunction(2,9),
  r2_set(i_r2_set),q_r(i_q_r),
  m2_set(i_m2_set),q_m(i_q_m),
  dt(i_dt),mode(i_mode),LatticeSize(i_LatticeSize),XYmax(i_XYmax),Freq(i_Freq),
  f_r2_f(i_dt,i_mode,i_XYmax,i_LatticeSize,i_Freq),
  f_m2_f(i_dt,i_mode,i_XYmax,i_LatticeSize,i_Freq)
{
  
}

eq_f::~eq_f(){}

double eq_f::value(const double *x){
  //  if(x[0]<=x[1]){
  //    return 2e19;
  //  }
  double r2_f = f_r2_f.value(x);
  double m2_f = f_m2_f.value(x);
  return q_r*(r2_set-r2_f)*(r2_set-r2_f)+q_m*(m2_set-m2_f)*(m2_set-m2_f);
}

void eq_f::gradient(const double *x,double *grad){
  double grad_r2_f[2];
  f_r2_f.gradient(x,grad_r2_f);
  double r2_f = f_r2_f.value(x);

  double grad_m2_f[2];
  f_m2_f.gradient(x,grad_m2_f);
  double m2_f = f_m2_f.value(x);
  grad[0] = -2*q_r*(r2_set-r2_f)*grad_r2_f[0]-2*q_m*(m2_set-m2_f)*grad_m2_f[0];
  grad[1] = -2*q_r*(r2_set-r2_f)*grad_r2_f[1]-2*q_m*(m2_set-m2_f)*grad_m2_f[1];
}

bool eq_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    r2_set = para_val;break;
  case 1:
    q_r = para_val;break;
  case 2:
    m2_set = para_val;break;
  case 3:
    q_m = para_val;break;
  case 4:
    mode = int(para_val);
    f_r2_f.parameterize(0,para_val);
    f_m2_f.parameterize(0,para_val);
    break;
  case 5:
    LatticeSize = int(para_val);
    f_r2_f.parameterize(3,para_val);
    f_m2_f.parameterize(3,para_val);
    break;
  case 6:
    XYmax = para_val;
    f_r2_f.parameterize(1,para_val);
    f_m2_f.parameterize(1,para_val);
    break;
  case 7:
    dt = para_val;
    f_r2_f.parameterize(4,para_val);
    f_m2_f.parameterize(4,para_val);
    break;
  case 8:
    Freq = para_val;
    f_r2_f.parameterize(2,para_val);
    f_m2_f.parameterize(2,para_val);
    break;
  default:
    cerr << "eq_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

bool eq_f::parameterize(int para_ID,double *para_array){
  f_r2_f.parameterize(para_ID,para_array);
  f_m2_f.parameterize(para_ID,para_array);
  return true;  
}
